#include <QElapsedTimer>
#include "neure.h"

neure::neure(QSerialPort *ser)
{
	m_ser = ser;
	m_header = false;
	m_online_cnt = 0;
	m_frame.clear();
	m_dev_online = false;
	m_r = m_g = m_b = false;

	connect(m_ser, &QSerialPort::readyRead, this, &neure::recvData);
	m_elapsedTimer.start();
}

neure::~neure()
{
	stop();
	polling(RGB_SENSOR_ID, REPORT_NONE, 0);
	polling(COLOR_SENSOR_ID, REPORT_NONE, 0);
	polling(ENV_SENSOR_ID, REPORT_NONE, 0);
	polling(GRAY_SENSOR_ID, REPORT_NONE, 0);
	polling(VISIBLE_REFLECT_SENSOR_ID, REPORT_NONE, 0);
	polling(IR_REFLECT_SENSOR_ID, REPORT_NONE, 0);
	polling(OBSTACLE_SENSOR_ID, REPORT_NONE, 0);
	polling(DC_CURRENT_SAMPLE_SENSOR_ID, REPORT_NONE, 0);

	m_dev_online = false;
	emit devOnlineUpdate(m_dev_online, 0);
}

void neure::assignDevId(void)
{
	QByteArray tx_data = QByteArray::fromHex("F0FF10000FF7");
	m_ser->write(tx_data);

	/*
	m_online_cnt++;
	if (m_online_cnt > 2)
	{
		m_dev_online = false;
		emit devOnlineUpdate(m_dev_online, 0);
	}
	*/
}

void neure::getFirmware(void)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = 0x12;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::send(QByteArray data)
{
	m_ser->write(data);
}

void neure::forward(quint8 speed)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = FORWARD_CMD_ID;
	tx_data[idx++] = speed;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx-1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::backward(quint8 speed)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = BACKWARD_CMD_ID;
	tx_data[idx++] = speed;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::motorControl(qint16 leftSpeed, qint16 rightSpeed)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = MOTOR_CTRL_CMD_ID;
	if (leftSpeed >= 0)
	{
		if (leftSpeed > 100)
		{
			leftSpeed = 100;
		}
		tx_data[idx++] = 0;
		tx_data[idx++] = leftSpeed;
	}
	else if (leftSpeed < -0)
	{
		if (leftSpeed < -100)
		{
			leftSpeed = -100;
		}
		leftSpeed = -leftSpeed;
		
		tx_data[idx++] = 1;
		tx_data[idx++] = leftSpeed;
	}
	if (rightSpeed >= 0)
	{
		if (rightSpeed > 100)
		{
			rightSpeed = 100;
		}
		tx_data[idx++] = 1;
		tx_data[idx++] = rightSpeed;
	}
	else if (rightSpeed < -0)
	{
		if (rightSpeed < -100)
		{
			rightSpeed = -100;
		}
		rightSpeed = -rightSpeed;

		tx_data[idx++] = 0;
		tx_data[idx++] = rightSpeed;
	}
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::stop(void)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = STOP_CMD_ID;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx-1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::leftTurn(void)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = LEFT_TURN_CMD_ID;
	tx_data[idx++] = 100;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::rightTurn(void)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = RIGHT_TURN_CMD_ID;
	tx_data[idx++] = 100;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

// Color
void neure::calibration(void)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = CALIBRATION_CMD_ID;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

void neure::sendGetCmd(get_cmd_id_e cmd)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = cmd;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
	m_start_tick = m_elapsedTimer.elapsed();
}

void neure::getRgb(void)
{
	sendGetCmd(GET_RGB);
}

void neure::getColor(void)
{
	sendGetCmd(GET_COLOR);
}

void neure::getEnv(void)
{
	sendGetCmd(GET_ENV);
}

void neure::getGray(void)
{
	sendGetCmd(GET_GRAY);
}

void neure::getVisibleReflect(void)
{
	sendGetCmd(GET_VISIBLE_REFLECT);
}

void neure::getIrReflect(void)
{
	sendGetCmd(GET_IR_REFLECT);
}

void neure::getObstacle(void)
{
	sendGetCmd(IF_OBSTACLE);
}

// set RGB
void neure::setRGB(bool r, bool g, bool b)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = SET_RGB_CMD_ID;
	tx_data[idx++] = r;
	tx_data[idx++] = g;
	tx_data[idx++] = b;
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
	m_start_tick = m_elapsedTimer.elapsed();
}
void neure::setR(void)
{
	m_r = !m_r;
	setRGB(m_r, m_g, m_b);
}
void neure::setG(void)
{
	m_g = !m_g;
	setRGB(m_r, m_g, m_b);
}void neure::setB(void)
{
	m_b = !m_b;
	setRGB(m_r, m_g, m_b);
}

// Polling
void neure::polling(reported_sensor_id_e id, reported_mode_e mode, quint32 cycle)
{
	int idx;
	if (!m_dev_online)
	{
		return;
	}

	idx = 0;
	QByteArray tx_data;
	tx_data[idx++] = FRAME_HEAD;
	tx_data[idx++] = m_dev_id;
	tx_data[idx++] = EXP_SRV;
	tx_data[idx++] = EXP_SUB_SRV;
	tx_data[idx++] = REPORT_CMD_ID;
	tx_data[idx++] = id;
	tx_data[idx++] = mode;
	QByteArray cycle_stream = build_long(cycle / 10);
	for (int i = 0; i < 5; i++)
	{
		tx_data[idx++] = (quint8)cycle_stream.at(i);
	}
	tx_data[idx++] = calcCheckSum(tx_data.mid(1, idx - 1));
	tx_data[idx++] = FRAME_END;
	m_ser->write(tx_data);
}

// rgb
void neure::pollingRgb(bool sw)
{
	polling(RGB_SENSOR_ID, REPORT_NONE, 0);
}

void neure::pollingRgb(void)
{
	polling(RGB_SENSOR_ID, REPORT_CHG, 0);
}

void neure::pollingRgb(quint32 reportCyclye)
{
	polling(RGB_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// color
void neure::pollingColor(bool sw)
{
	polling(COLOR_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingColor(void)
{
	polling(COLOR_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingColor(quint32 reportCyclye)
{
	polling(COLOR_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// env
void neure::pollingEnv(bool sw)
{
	polling(ENV_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingEnv(void)
{
	polling(ENV_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingEnv(quint32 reportCyclye)
{
	polling(ENV_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// gray
void neure::pollingGray(bool sw)
{
	polling(GRAY_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingGray(void)
{
	polling(GRAY_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingGray(quint32 reportCyclye)
{
	polling(GRAY_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// visible reflect
void neure::pollingVisibleReflect(bool sw)
{
	polling(VISIBLE_REFLECT_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingVisibleReflect(void)
{
	polling(VISIBLE_REFLECT_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingVisibleReflect(quint32 reportCyclye)
{
	polling(VISIBLE_REFLECT_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// ir reflect
void neure::pollingIrReflect(bool sw)
{
	polling(IR_REFLECT_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingIrReflect(void)
{
	polling(IR_REFLECT_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingIrReflect(quint32 reportCyclye)
{
	polling(IR_REFLECT_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// obstacle
void neure::pollingObstacle(bool sw)
{
	polling(OBSTACLE_SENSOR_ID, REPORT_NONE, 0);
}
void neure::pollingObstacle(void)
{
	polling(OBSTACLE_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingObstacle(quint32 reportCyclye)
{
	polling(OBSTACLE_SENSOR_ID, REPORT_PRD, reportCyclye);
}

// dc current
void neure::pollingDcCurrent(bool sw)
{
	if (!sw)
	{
		polling(DC_CURRENT_SAMPLE_SENSOR_ID, REPORT_NONE, 0);
	}
	else
	{
		polling(DC_CURRENT_SAMPLE_SENSOR_ID, REPORT_PRD, 500);
	}
}
void neure::pollingDcCurrent(void)
{
	polling(DC_CURRENT_SAMPLE_SENSOR_ID, REPORT_CHG, 0);
}
void neure::pollingDcCurrent(quint32 reportCyclye)
{
	polling(DC_CURRENT_SAMPLE_SENSOR_ID, REPORT_PRD, reportCyclye);
}

void neure::recvData(void)
{
	QByteArray buf;

	while (m_ser->bytesAvailable())
	{
		buf = m_ser->readLine();
		if (!buf.isEmpty())
		{
			for (int i = 0; i < buf.length(); i++)
			{
				if (m_header)
				{
					m_frame.append(buf.at(i));
					if (FRAME_END == (quint8)buf.at(i))
					{
						m_header = false;
						m_end_tick = m_elapsedTimer.elapsed();
						proFrame(m_frame);
					}
				}
				else
				{
					if (FRAME_HEAD == (quint8)buf.at(i))
					{
						m_frame.clear();
						m_frame.append(buf.at(i));
						m_header = true;
					}
				}
			}
			emit logInfo(buf);
		}
	}
}

quint8 neure::calcCheckSum(QByteArray frameData)
{
	quint8 sum;

	sum = 0;
	for (int i = 0; i < frameData.length(); i++)
	{
		sum += (quint8)frameData[i];
	}
	return sum = sum & 0x7F;
}

bool neure::checkSum(QByteArray frame)
{
	int i = FRAME_CHECK_START_IDX;
	quint8 sum;

	sum = 0;
	for (; i < frame.length() - 2; i++)
	{
		sum += (quint8)frame[i];
	}
	sum = sum & 0x7F;
	
	if (sum == (quint8)frame[i])
	{
		return true;
	}
	else
	{
		return false;
	}
}

quint16 neure::readBigShort( QByteArray streamData )
{
	quint8 *p;
	quint16 ret;

	p = (quint8 *)&ret;
	p[0] = ((quint8)streamData[0] & 0x7f) | ((quint8)streamData[1]<<7);
	p[1] = ((quint8)streamData[1] >> 1) & 0x7f;

	return ret;
}

QByteArray neure::build_long(quint32 struct_data)
{
	quint8 *p;
	QByteArray ret;

	p = (quint8 *)&struct_data;
	ret[0] = p[0] & 0x7f;
	ret[1] = ((p[1] << 1) | (p[0] >> 7)) & 0x7f;
	ret[2] = ((p[2] << 2) | (p[1] >> 6)) & 0x7f;
	ret[3] = ((p[3] << 3) | (p[2] >> 5)) & 0x7f;
	ret[4] =                (p[3] >> 4)  & 0x7f;

	return ret;
}

void neure::proFrame(QByteArray frame)
{
	quint8 srv_id;
	emit logInfo(frame.toHex(' '));
	
	if (!checkSum(frame))
	{
		return;
	}

	srv_id = (quint8)frame[FRAME_SRV_IDX];

	// Common neure service
	if (srv_id < 0x20)
	{
		switch (srv_id)
		{
		case DEV_ID:
			proAssignDevId(frame);
			break;

		case FIRMWARE:
			proGetFirmware(frame);
			break;
		}
	}

	else if (EXP_SRV == srv_id)
	{
		quint8 subSrv = (quint8)frame[3];
		if (EXP_SUB_SRV == subSrv)
		{
			quint8 cmd = frame[4];
			switch (cmd)
			{
			case GET_RGB:
				proGetRgb(frame);
				break;

			case GET_COLOR:
				proGetColor(frame);
				break;

			case GET_ENV:
				proGetEnv(frame);
				break;

			case GET_GRAY:
				proGetGray(frame);
				break;

			case GET_VISIBLE_REFLECT:
				proGetVisibleReflect(frame);
				break;

			case GET_IR_REFLECT:
				proGetIrReflect(frame);
				break;

			case IF_OBSTACLE:
				proGetObstacle(frame);
				break;

			case DC_CURRENT:
				proGetDcCurrent(frame);
				break;
			}
		}
	}
}

void neure::proAssignDevId(QByteArray frame)
{
	m_online_cnt = 0;
	m_dev_online = true;
	if (EXP_SRV == (quint8)frame[3] && EXP_SUB_SRV == (quint8)frame[4])
	{
		m_dev_id = (quint8)frame[1];
		emit devOnlineUpdate(m_dev_online, m_dev_id);
		getFirmware();
	}
}

void neure::proGetFirmware(QByteArray frame)
{
	emit firmwareUpdate(QString::fromLatin1(frame.mid(5,2).toHex()));
}

void neure::proGetRgb(QByteArray frame)
{
	quint16 r, g, b;

	r = readBigShort(frame.mid(5, 2));
	g = readBigShort(frame.mid(7, 2));
	b = readBigShort(frame.mid(9, 2));

	emit rgbUpdate(r, g, b);
}

void neure::proGetColor(QByteArray frame)
{
	quint8 color_id;
	quint16 r, g, b;
	QString color_id_str;

	color_id = (quint8)frame.at(5);
	switch (color_id)
	{
	case WHITE:
		r = 255; g = 255; b = 255;
		color_id_str = "white";
	break;

	case PINK:
		r = 255; g = 0; b = 255;
		color_id_str = "pink";
		break;

	case RED:
		r = 255; g = 0; b = 0;
		color_id_str = "red";
		break;

	case ORANGE:
		r = 255; g = 128; b = 0;
		color_id_str = "orange";
		break;

	case YELLOW:
		r = 255; g = 255; b = 0;
		color_id_str = "yellow";
		break;

	case GREEN:
		r = 0; g = 255; b = 0;
		color_id_str = "green";
		break;

	case CYAN:
		r = 0; g = 255; b = 255;
		color_id_str = "cyan";
		break;

	case BLUE:
		r = 0; g = 0; b = 255;
		color_id_str = "blue";
		break;

	case PURPLE:
		r = 255; g = 0; b = 255;
		color_id_str = "purple";
		break;

	case BLACK:
		r = 0; g = 0; b = 0;
		color_id_str = "black";
		break;

	default:
		break;
	}
	emit colorUpdate(r, g, b, color_id_str);
}

void neure::proGetEnv(QByteArray frame)
{
	emit envUpdate((quint8)frame.at(5));
}

void neure::proGetGray(QByteArray frame)
{
	emit grayUpdate((quint8)frame.at(5));
}

void neure::proGetVisibleReflect(QByteArray frame)
{
	emit visibleReflectUpdate((quint8)frame.at(5));
}

void neure::proGetIrReflect(QByteArray frame)
{
	emit irReflectUpdate((quint8)frame.at(5));
}

void neure::proGetObstacle(QByteArray frame)
{
	emit obstacleUpdate((quint8)frame.at(5));
}

void neure::proGetDcCurrent(QByteArray frame)
{
	quint16 l, r;

	l = readBigShort(frame.mid(5, 2));
	r = readBigShort(frame.mid(7, 2));

	emit dcCurrentUpdate(l, r);
}